#include "Matrix3.h"

Matrix3::Matrix3()
{
	_elements.e[0][0] = 1.0;
	_elements.e[0][1] = 0.0;
	_elements.e[0][2] = 0.0;
	_elements.e[0][3] = 0.0;
	_elements.e[1][0] = 0.0;
	_elements.e[1][1] = 1.0;
	_elements.e[1][2] = 0.0;
	_elements.e[1][3] = 0.0;
	_elements.e[2][0] = 0.0;
	_elements.e[2][1] = 0.0;
	_elements.e[2][2] = 1.0;
	_elements.e[2][3] = 0.0;
	_elements.e[3][0] = 0.0;
	_elements.e[3][1] = 0.0;
	_elements.e[3][2] = 0.0;
	_elements.e[3][3] = 1.0;
}
Matrix3::Matrix3(const Matrix3 &mat)
{
	_elements.e[0][0] = mat.getElements().e[0][0];
	_elements.e[0][1] = mat.getElements().e[0][1];
	_elements.e[0][2] = mat.getElements().e[0][2];
	_elements.e[0][3] = mat.getElements().e[0][3];
	_elements.e[1][0] = mat.getElements().e[1][0];
	_elements.e[1][1] = mat.getElements().e[1][1];
	_elements.e[1][2] = mat.getElements().e[1][2];
	_elements.e[1][3] = mat.getElements().e[1][3];
	_elements.e[2][0] = mat.getElements().e[2][0];
	_elements.e[2][1] = mat.getElements().e[2][1];
	_elements.e[2][2] = mat.getElements().e[2][2];
	_elements.e[2][3] = mat.getElements().e[2][3];
	_elements.e[3][0] = mat.getElements().e[3][0];
	_elements.e[3][1] = mat.getElements().e[3][1];
	_elements.e[3][2] = mat.getElements().e[3][2];
	_elements.e[3][3] = mat.getElements().e[3][3];
}

Matrix3::Matrix3( float e11,float e12,float e13,
			float e21,float e22,float e23, 
			float e31,float e32,float e33 )
{
	_elements.e[0][0] = e11;
	_elements.e[0][1] = e12;
	_elements.e[0][2] = e13;
	_elements.e[0][3] = 0.0;
	_elements.e[1][0] = e21;
	_elements.e[1][1] = e22;
	_elements.e[1][2] = e23;
	_elements.e[1][3] = 0.0;
	_elements.e[2][0] = e31;
	_elements.e[2][1] = e32;
	_elements.e[2][2] = e33;
	_elements.e[2][3] = 0.0;
	_elements.e[3][0] = 0.0;
	_elements.e[3][1] = 0.0;
	_elements.e[3][2] = 0.0;
	_elements.e[3][3] = 1.0;
}

Matrix3::Matrix3( float e11,float e12,float e13, float e14,
			float e21,float e22,float e23, float e24,
			float e31,float e32,float e33, float e34,
			float e41,float e42,float e43, float e44 )
{
	_elements.e[0][0] = e11;
	_elements.e[0][1] = e12;
	_elements.e[0][2] = e13;
	_elements.e[0][3] = e14;
	_elements.e[1][0] = e21;
	_elements.e[1][1] = e22;
	_elements.e[1][2] = e23;
	_elements.e[1][3] = e24;
	_elements.e[2][0] = e31;
	_elements.e[2][1] = e32;
	_elements.e[2][2] = e33;
	_elements.e[2][3] = e34;
	_elements.e[3][0] = e41;
	_elements.e[3][1] = e42;
	_elements.e[3][2] = e43;
	_elements.e[3][3] = e44;
}

Matrix3::Matrix3( MatrixElements4x4 elements )
{
	_elements.e[0][0] = elements.e[0][0];
	_elements.e[0][1] = elements.e[0][1];
	_elements.e[0][2] = elements.e[0][2];
	_elements.e[0][3] = elements.e[0][3];
	_elements.e[1][0] = elements.e[1][0];
	_elements.e[1][1] = elements.e[1][1];
	_elements.e[1][2] = elements.e[1][2];
	_elements.e[1][3] = elements.e[1][3];
	_elements.e[2][0] = elements.e[2][0];
	_elements.e[2][1] = elements.e[2][1];
	_elements.e[2][2] = elements.e[2][2];
	_elements.e[2][3] = elements.e[2][3];
	_elements.e[3][0] = elements.e[3][0];
	_elements.e[3][1] = elements.e[3][1];
	_elements.e[3][2] = elements.e[3][2];
	_elements.e[3][3] = elements.e[3][3];
}

		
//Getter/Setter
MatrixElements4x4 Matrix3::getElements() const
{
	return _elements;
}
void Matrix3::setElements( MatrixElements4x4 elements)
{
	_elements = elements;
}

//Overload Operators
Matrix3 Matrix3::operator * (const Matrix3 &mat2) const
{
	Matrix3 tmpmat(
						this->_elements.e[0][0] * mat2._elements.e[0][0] + this->_elements.e[0][1] * mat2._elements.e[1][0] + 
							this->_elements.e[0][2] * mat2._elements.e[2][0] + this->_elements.e[0][3] * mat2._elements.e[3][0],
						this->_elements.e[0][0] * mat2._elements.e[0][1] + this->_elements.e[0][1] * mat2._elements.e[1][1] + 
							this->_elements.e[0][2] * mat2._elements.e[2][1] + this->_elements.e[0][3] * mat2._elements.e[3][1],
						this->_elements.e[0][0] * mat2._elements.e[0][2] + this->_elements.e[0][1] * mat2._elements.e[1][2] + 
							this->_elements.e[0][2] * mat2._elements.e[2][2] + this->_elements.e[0][3] * mat2._elements.e[3][2],
						this->_elements.e[0][0] * mat2._elements.e[0][3] + this->_elements.e[0][1] * mat2._elements.e[1][3] + 
							this->_elements.e[0][2] * mat2._elements.e[2][3] + this->_elements.e[0][3] * mat2._elements.e[3][3],

						this->_elements.e[1][0] * mat2._elements.e[0][0] + this->_elements.e[1][1] * mat2._elements.e[1][0] + 
							this->_elements.e[1][2] * mat2._elements.e[2][0] + this->_elements.e[1][3] * mat2._elements.e[3][0],
						this->_elements.e[1][0] * mat2._elements.e[0][1] + this->_elements.e[1][1] * mat2._elements.e[1][1] + 
							this->_elements.e[1][2] * mat2._elements.e[2][1] + this->_elements.e[1][3] * mat2._elements.e[3][1],
						this->_elements.e[1][0] * mat2._elements.e[0][2] + this->_elements.e[1][1] * mat2._elements.e[1][2] + 
							this->_elements.e[1][2] * mat2._elements.e[2][2] + this->_elements.e[1][3] * mat2._elements.e[3][2],
						this->_elements.e[1][0] * mat2._elements.e[0][3] + this->_elements.e[1][1] * mat2._elements.e[1][3] + 
							this->_elements.e[1][2] * mat2._elements.e[2][3] + this->_elements.e[1][3] * mat2._elements.e[3][3],

						this->_elements.e[2][0] * mat2._elements.e[0][0] + this->_elements.e[2][1] * mat2._elements.e[1][0] + 
							this->_elements.e[2][2] * mat2._elements.e[2][0] + this->_elements.e[2][3] * mat2._elements.e[3][0],
						this->_elements.e[2][0] * mat2._elements.e[0][1] + this->_elements.e[2][1] * mat2._elements.e[1][1] + 
							this->_elements.e[2][2] * mat2._elements.e[2][1] + this->_elements.e[2][3] * mat2._elements.e[3][1],
						this->_elements.e[2][0] * mat2._elements.e[0][2] + this->_elements.e[2][1] * mat2._elements.e[1][2] + 
							this->_elements.e[2][2] * mat2._elements.e[2][2] + this->_elements.e[2][3] * mat2._elements.e[3][2],
						this->_elements.e[2][0] * mat2._elements.e[0][3] + this->_elements.e[2][1] * mat2._elements.e[1][3] + 
							this->_elements.e[2][2] * mat2._elements.e[2][3] + this->_elements.e[2][3] * mat2._elements.e[3][3],

						this->_elements.e[3][0] * mat2._elements.e[0][0] + this->_elements.e[3][1] * mat2._elements.e[1][0] + 
							this->_elements.e[3][2] * mat2._elements.e[2][0] + this->_elements.e[3][3] * mat2._elements.e[3][0],
						this->_elements.e[3][0] * mat2._elements.e[0][1] + this->_elements.e[3][1] * mat2._elements.e[1][1] + 
							this->_elements.e[3][2] * mat2._elements.e[2][1] + this->_elements.e[3][3] * mat2._elements.e[3][1],
						this->_elements.e[3][0] * mat2._elements.e[0][2] + this->_elements.e[3][1] * mat2._elements.e[1][2] + 
							this->_elements.e[3][2] * mat2._elements.e[2][2] + this->_elements.e[3][3] * mat2._elements.e[3][2],
						this->_elements.e[3][0] * mat2._elements.e[0][3] + this->_elements.e[3][1] * mat2._elements.e[1][3] + 
							this->_elements.e[3][2] * mat2._elements.e[2][3] + this->_elements.e[3][3] * mat2._elements.e[3][3]

	);
	return tmpmat;
}

//Overload Operators
Matrix3 Matrix3::operator * (const float& scal) const
{
	Matrix3 tmpmat(
						 	scal * this->getElements().e[0][0], scal * this->getElements().e[0][1],scal * this->getElements().e[0][2],
							scal * this->getElements().e[1][0], scal * this->getElements().e[1][1],scal * this->getElements().e[1][2],
							scal * this->getElements().e[2][0], scal * this->getElements().e[2][1],scal * this->getElements().e[2][2]

	);
	return tmpmat;
}

Vector3 Matrix3::operator * (const Vector3 &vec) const
{
	Vector3 tmpVec(
					vec.getX() * this->getElements().e[0][0] + vec.getY() * this->getElements().e[0][1] + 
						vec.getZ() * this->getElements().e[0][2] + vec.getW() * this->getElements().e[0][3],
					vec.getX() * this->getElements().e[1][0] + vec.getY() * this->getElements().e[1][1] + 
						vec.getZ() * this->getElements().e[1][2] + vec.getW() * this->getElements().e[1][3],
					vec.getX() * this->getElements().e[2][0] + vec.getY() * this->getElements().e[2][1] + 
						vec.getZ() * this->getElements().e[2][2] + vec.getW() * this->getElements().e[2][3],
					vec.getX() * this->getElements().e[3][0] + vec.getY() * this->getElements().e[3][1] + 
						vec.getZ() * this->getElements().e[3][2] + vec.getW() * this->getElements().e[3][3]
	);
	return tmpVec;
}


Matrix3 Matrix3::operator + (const Matrix3 &mat2) const
{
	Matrix3 mat(  
      this->_elements.e[0][0] + mat2.getElements().e[0][0],
			this->_elements.e[0][1] + mat2.getElements().e[0][1],
			this->_elements.e[0][2] + mat2.getElements().e[0][2],
			this->_elements.e[0][3] + mat2.getElements().e[0][3],

      this->_elements.e[1][0] + mat2.getElements().e[1][0],
			this->_elements.e[1][1] + mat2.getElements().e[1][1],
			this->_elements.e[1][2] + mat2.getElements().e[1][2],
			this->_elements.e[1][3] + mat2.getElements().e[1][3],

      this->_elements.e[2][0] + mat2.getElements().e[2][0],
			this->_elements.e[2][1] + mat2.getElements().e[2][1],
			this->_elements.e[2][2] + mat2.getElements().e[2][2],
			this->_elements.e[2][3] + mat2.getElements().e[2][3],

      this->_elements.e[3][0] + mat2.getElements().e[3][0],
			this->_elements.e[3][1] + mat2.getElements().e[3][1],
			this->_elements.e[3][2] + mat2.getElements().e[3][2],
			this->_elements.e[3][3] + mat2.getElements().e[3][3]
		);
	return mat;
}

Matrix3 Matrix3::operator - (const Matrix3 &mat2) const
{
	Matrix3 mat(  
      this->_elements.e[0][0] - mat2.getElements().e[0][0],
			this->_elements.e[0][1] - mat2.getElements().e[0][1],
			this->_elements.e[0][2] - mat2.getElements().e[0][2],
			this->_elements.e[0][3] - mat2.getElements().e[0][3],

      this->_elements.e[1][0] - mat2.getElements().e[1][0],
			this->_elements.e[1][1] - mat2.getElements().e[1][1],
			this->_elements.e[1][2] - mat2.getElements().e[1][2],
			this->_elements.e[1][3] - mat2.getElements().e[1][3],

      this->_elements.e[2][0] - mat2.getElements().e[2][0],
			this->_elements.e[2][1] - mat2.getElements().e[2][1],
			this->_elements.e[2][2] - mat2.getElements().e[2][2],
			this->_elements.e[2][3] - mat2.getElements().e[2][3],

      this->_elements.e[3][0] - mat2.getElements().e[3][0],
			this->_elements.e[3][1] - mat2.getElements().e[3][1],
			this->_elements.e[3][2] - mat2.getElements().e[3][2],
			this->_elements.e[3][3] - mat2.getElements().e[3][3]
		);
	return mat;
}

float* Matrix3::getElementArray() const
{
	float *eles = new float[16];
	int i = 0;
	for( int spalte = 0;spalte < 4;spalte++ )
	{
		for( int zeile = 0;zeile < 4;zeile++ )
		{
			eles[i] = _elements.e[zeile][spalte]; 
			i++;
		}
	}
	return eles;
}

void Matrix3::LoadIdentity()
{
	_elements.e[0][0] = 1.0;
	_elements.e[0][1] = 0.0;
	_elements.e[0][2] = 0.0;
	_elements.e[0][3] = 0.0;
	_elements.e[1][0] = 0.0;
	_elements.e[1][1] = 1.0;
	_elements.e[1][2] = 0.0;
	_elements.e[1][3] = 0.0;
	_elements.e[2][0] = 0.0;
	_elements.e[2][1] = 0.0;
	_elements.e[2][2] = 1.0;
	_elements.e[2][3] = 0.0;
	_elements.e[3][0] = 0.0;
	_elements.e[3][1] = 0.0;
	_elements.e[3][2] = 0.0;
	_elements.e[3][3] = 1.0;	
}

Vector3 Matrix3::MultNormal( Vector3 vec ) const
{
	Vector3 tmpVec = MultNormalNotNormalize (vec);
	tmpVec.normalize();
	return tmpVec;	
}

Vector3 Matrix3::MultNormalNotNormalize( Vector3 vec ) const
{
	Vector3 tmpVec(
					vec.getX() * this->getElements().e[0][0] + vec.getY() * this->getElements().e[0][1] + 
						vec.getZ() * this->getElements().e[0][2],
					vec.getX() * this->getElements().e[1][0] + vec.getY() * this->getElements().e[1][1] + 
						vec.getZ() * this->getElements().e[1][2],
					vec.getX() * this->getElements().e[2][0] + vec.getY() * this->getElements().e[2][1] + 
						vec.getZ() * this->getElements().e[2][2],1.0
	);
	return tmpVec;	
}


float Matrix3::getDeterminant() const
{
	//
	// | e11 e12 e13 |e11 e12
  //      \   X   X    / 
	// | e21 e22 e23 |e21 e22
  //      /   X   X    \   
	// | e31 e32 e33 |e31 e32
	//
	return ( 
				( _elements.e[0][0] * _elements.e[1][1] * _elements.e[2][2]  ) +
				( _elements.e[0][1] * _elements.e[1][2] * _elements.e[2][0]  ) +
				( _elements.e[0][2] * _elements.e[1][0] * _elements.e[2][1]  ) -
				( _elements.e[2][0] * _elements.e[1][1] * _elements.e[0][2]  ) -
				( _elements.e[2][1] * _elements.e[1][2] * _elements.e[0][0]  ) -
				( _elements.e[2][2] * _elements.e[1][0] * _elements.e[0][1]  ) 
	);
}

Matrix3 Matrix3::getInverse() const
{
	// determinante ausrechnen
	float det = getDeterminant();
	if( GeoMath::Abs(det) < GeoMath::EPS )
		throw " |-X- Exception -X-| Determinant is 0, cannot calc the inverse!";

	// inverse der determinante
	det = 1.0/det;

	Matrix3 mat(
		Matrix2( _elements.e[1][1],_elements.e[1][2],_elements.e[2][1],_elements.e[2][2]).getDeterminant(),
		Matrix2( _elements.e[0][2],_elements.e[0][1],_elements.e[2][2],_elements.e[2][1]).getDeterminant(),
		Matrix2( _elements.e[0][1],_elements.e[0][2],_elements.e[1][1],_elements.e[1][2]).getDeterminant(),
		Matrix2( _elements.e[1][2],_elements.e[1][0],_elements.e[2][2],_elements.e[2][0]).getDeterminant(),
		Matrix2( _elements.e[0][0],_elements.e[0][2],_elements.e[2][0],_elements.e[2][2]).getDeterminant(),
		Matrix2( _elements.e[0][2],_elements.e[0][0],_elements.e[1][2],_elements.e[1][0]).getDeterminant(),
		Matrix2( _elements.e[1][0],_elements.e[1][1],_elements.e[2][0],_elements.e[2][1]).getDeterminant(),
		Matrix2( _elements.e[0][1],_elements.e[0][0],_elements.e[2][1],_elements.e[2][0]).getDeterminant(),
		Matrix2( _elements.e[0][0],_elements.e[0][1],_elements.e[1][0],_elements.e[1][1]).getDeterminant()
	);

	mat = mat * det;

	return mat;
}

void Matrix3::InverseMatrix()
{
	this->setElements( this->getInverse().getElements() );
}


ostream & operator<< (ostream &o, const Matrix3 &mat)
{
	o 	<< "["<<mat.getElements().e[0][0]<<"] ["<<mat.getElements().e[0][1]<<"] ["<<mat.getElements().e[0][2]<<"] ["<<mat.getElements().e[0][3]<<"]"<<endl
	<< "["<<mat.getElements().e[1][0]<<"] ["<<mat.getElements().e[1][1]<<"] ["<<mat.getElements().e[1][2]<<"] ["<<mat.getElements().e[1][3]<<"]"<<endl
	<< "["<<mat.getElements().e[2][0]<<"] ["<<mat.getElements().e[2][1]<<"] ["<<mat.getElements().e[2][2]<<"] ["<<mat.getElements().e[2][3]<<"]"<<endl
	<< "["<<mat.getElements().e[3][0]<<"] ["<<mat.getElements().e[3][1]<<"] ["<<mat.getElements().e[3][2]<<"] ["<<mat.getElements().e[3][3]<<"]"<<endl;
	return o;
};




